/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
 * This object implements a generalized UKF. The user must provide
 * the necessary input data (see constructor).
 *                      
 * FILENAME:
 * ukf.h
 *
 * PURPOSE:
 * To implement the Unscented Kalman Filter for use in state estimation.
 *
 * CREATED:
 * 10/2009 by Maciej Stachura
 *
 * LAST MODIFIED:
 * $Date: 2009/10/13 $ * $Revision: 1.0 $
 * $Author: stachura $
*/ 

#include <stdint.h>

#include "ukf.h"

//constructor
UKF::UKF ()
{
  bet = 2;
}

//constructor
UKF::UKF ( Matrix _x, Matrix _P, Matrix (*_f)(Matrix,FiltData_t), Matrix (*_g)(Matrix,FiltData_t), FiltData_t _param  )
{
  // Starting Matrices
  x = _x;
  P = _P;
  // State and measurement Equations
  f = _f;
  g = _g;
  // UKF Parameters
  param = _param;
  bet = 2;
}

UKF::~UKF (){
//	cout << "Destroyed!\n";
}

void UKF::time_update()
{

	//if(f == NULL) {cout << "uninitialized state update function" << endl; return;}
  // THE SIGMA POINTS (Prediction Step)
  // Parameters
  n   = x.length();
  N   = (T)(x.length());
  kap = 3 - N;
  lam = pow(param.alpha,2)*(N + kap) - N;
  gam = sqrt(N + kap);
  
  // Weights
  Wom = lam / (N + lam);
  Woc = Wom + 1 - pow(param.alpha,2) + bet;
  Wim = 1 / (2*(N + lam));
  Wic = Wim;
  
  // Compute the sigma points, X_0 and X.
  rep.SetSize(1,n);
  rep.Ones();

  X_0 = x;
  X1  = x*rep + gam*P.mSqrt();
  X2  = x*rep - gam*P.mSqrt();
  
  // Propagate X
  X_0 = f(X_0, param);
  for(size_t i=0;i<n;i++){
      X1.setCol( i, f(X1.getCol(i), param) );
      X2.setCol( i, f(X2.getCol(i), param) );
  }
  
  // Time update
  x_bar = Wom*X_0;
  for (size_t i=0;i<n;i++)
    x_bar += Wim*(X1.getCol(i) + X2.getCol(i));

  P_bar = param.Q + Woc*(X_0 - x_bar) * ~(X_0 - x_bar);
  for (size_t i=0;i<n;i++)
      P_bar += Wic*(X1.getCol(i) - x_bar) * ~(X1.getCol(i) - x_bar) 
        + Wic*(X2.getCol(i) - x_bar) * ~(X2.getCol(i) - x_bar);

  // Output to the state in case there is no measurement
  x = x_bar;
  P = P_bar;
}

// ALWAYS RUN THE TIME UPDATE BEFORE THIS, OTHERWISE IT MAKES NO SENSE STUPID
void UKF::meas_update(Matrix z)
{
  // Compute new sigma points
  X_0 = x;
  X1  = x*rep + gam*P.mSqrt();
  X2  = x*rep - gam*P.mSqrt();

  // Compute the measurements
  Z_0  = g(X_0, param);
  Z1.SetSize(z.length(),n);
  Z2.SetSize(z.length(),n);
  for(size_t i=0;i<n;i++){
    Z1.setCol( i, g(X1.getCol(i), param) );
    Z2.setCol( i, g(X2.getCol(i), param) );
  }
 
  z_bar = Wom*Z_0;
  for (size_t i=0;i<n;i++)
    z_bar += Wim*(Z1.getCol(i) + Z2.getCol(i));

  // Compute innovation and cross-correlation covariance
  Pzz = param.R + Woc*(Z_0 - z_bar)*~(Z_0 - z_bar);
  Pxz = Woc*(X_0 - x) * ~(Z_0 - z_bar);
  for (size_t i=0;i<n;i++){
    Pzz += Wic * (Z1.getCol(i) - z_bar) * ~(Z1.getCol(i) - z_bar)
      + Wic * (Z2.getCol(i) - z_bar) * ~(Z2.getCol(i) - z_bar);
    Pxz += Wic * (X1.getCol(i) - x) * ~(Z1.getCol(i) - z_bar)
      + Wic * (X2.getCol(i) - x) * ~(Z2.getCol(i) - z_bar);
  }

  // Measurement Update
  K = Pxz * Pzz.Inv();
  x = x + K * (z - z_bar);
  P = P - K * Pzz * ~K;
}


